/**
 * @file can_protocol.c
 * @author YangSL (dalin@main.com)
 * @brief SDO段，块解析文件
 * @version 0.0.1
 * @date 2022-09-28
 * 
 * @copyright Copyright (c) 2022
 * 
 */

#include "can_protocol.h"
#include "bsp_can_bus.h"
#include "can_interface.h"

scan_run_msg can_run_msg;


/************************************************** 共用**************************************************/
static void clean_cmd(void);
/* 发送错误帧 */
static int8_t send_err(int fun_id,int8_t da_id, sindex m_index, int32_t err_code);
/* 发送can数据 */
static int can_send(ucan_frame *canFrame);
/* 发送sdo数据 */
int8_t sdo_send(sCanIndex index,uint8* data,uint8 len);
/* 任务末端要清除程序过程命令 */
static void clean_cmd(void);
/* sdo写数据回调 */
static int8_t sdo_write(uint8_t port, sindex m_index,int8_t *data,int16_t len);
/* SDO读数据 */
static int8_t sdo_read(uint8_t port, sindex m_index,int8_t *data,int16_t *len);
/* ack响应等待 */
static int8_t as_ser_wait_ack(void);
/* SDO终止协议 */
int8_t sdo_stop(ucan_frame *can_msg);
/************************************************** 客服端 **************************************************/
/* 作为客户端：段下载启动协议 */
static int8_t as_clt_segment_load_init(int8_t da_id,sindex m_index,uint8_t is_fast,int8_t *data,int8_t len);
/* 作为客户端：SDO段下载协议 */
static int8_t as_clt_segment_load(int8_t da_id,sindex m_index,int8_t *data,int8_t len);
/* 作为客户端：SDO段上传协议 */
static int8_t as_clt_segment_up_requests(int8_t da_id,sindex m_index);

/* 作为客户端：块传输初始化 */
static int8_t block_send_init(int8_t da_id,sindex m_index,int32_t len);
/* 作为客户端：子块传输 */
static int8_t block_child_send(int8_t da_id,int8_t *data,uint8_t len);
/* 作为客户端：块传输结束 */
static int8_t block_send_end(int8_t da_id,int16_t crc_data,bool crc_check);
/* 块上传输初始化 */
static int8_t block_up_init(int8_t da_id,sindex m_index,uint8_t blk_size);

/* 作为客户端：段上传初始化 ACK */
static int8_t as_ser_segment_load_init_ack(uint8_t port, ucan_frame *can_msg);
/* 作为客户端：分段段下载 ACK */
static int8_t as_ser_segment_load_ack(uint8_t port, ucan_frame *can_msg);
/* 作为客户端：段上传初始化 ACK */
static int8_t as_ser_segment_up_init_ack(uint8_t port, ucan_frame *can_msg);
/* 作为客户端：分段上传 ACK */
static int8_t as_ser_segment_up_ack(uint8_t port, ucan_frame *can_msg);
/* 作为客户端时响应服务器命令 */
static int8_t clt_sdo_process(uint8_t port, ucan_frame *can_msg);
/************************************************** 服务器 **************************************************/
/* 作为服务器时：SDO段下载启动协议 */
static int8_t as_ser_segment_load_init(uint8_t port, ucan_frame *can_msg);
/* 作为服务器时：SDO分段下载协议 */
static int8_t as_ser_segment_load(uint8_t port, ucan_frame *can_msg);
/* 作为服务器时：SDO段上传启动协议 */
static int8_t as_ser_segment_up_init(uint8_t port, ucan_frame *can_msg);
/* 作为服务器时：SDO分段上传协议 */
static int8_t as_ser_segment_up(uint8_t port, ucan_frame *can_msg);

/* 作为服务器时：SDO块下载协议 */
static int8_t as_ser_block_load(uint8_t port, ucan_frame *can_msg);

/* 段发送接口 */
static int8_t segment_send(int8_t da_id,int8_t *data , uint16_t len);
/* 作为服务器时响应客户端命令 */
static int8_t ser_sdo_process(uint8_t port, ucan_frame *can_msg);

/************************************************** 分层接口 **************************************************/
/* 客户端与服务器解释 */
static int8_t sdo_process(uint8_t port, ucan_frame *can_msg);
/* 检测当前节点 */
static int8_t sdo_analysis(uint16_t comp_type,ucan_frame *can_msg);
/* 对外接口：解析COB-ID类型 */
int8_t protocol_analysis(ucan_frame *can_pkg);



/**
 * @brief 发送错误帧
 * 
 * @param fun_id 
 * @param da_id 
 * @param m_index 
 * @param err_code 
 * @return int8_t 
 */
static int8_t send_err(int fun_id,int8_t da_id, sindex m_index, int32_t err_code)
{
    ucan_frame can_msg ;
    int8_t ret = 0 ;
    
    can_msg.byte.m_cob_id.byte.m_DA_ID  = da_id ;
    can_msg.byte.m_cob_id.byte.m_SA_ID  = CUR_MASTER_ID ;
    can_msg.byte.m_cob_id.byte.m_FUN_ID = fun_id ;
    can_msg.byte.m_length = 8 ;
    can_msg.byte.data.type1.cmd_data.stop_cmd.cs = e_clt_stop_cmd ;
    can_msg.byte.data.type1.m_index = htons(m_index.index);
    can_msg.byte.data.type1.m_subindex = m_index.subindex ;
    can_msg.byte.data.type1.data.data32 = htonl(err_code);
    ret = can_send(&can_msg);
    
    return ret ;
}


/**
 * @brief 
 * 
 * @param index 
 * @param data 
 * @param len 
 * @return int8_t 
 **/
int8_t sdo_send(sCanIndex index,uint8* data,uint8 len)
{
    ucan_frame canFrame;
	bool ret = false ;
    int i = 0 ;
    if(len < 5)//快传
    {
        canFrame.byte.m_length = 4+len ;
        canFrame.byte.m_cob_id.byte.m_SA_ID = CUR_MASTER_ID ;
        canFrame.byte.m_cob_id.byte.m_DA_ID = IPC_ID ;
        canFrame.byte.m_cob_id.byte.m_FUN_ID = SDO_RX_ID ;
        canFrame.byte.data.type1.cmd_data.load_sect_init.ccs = e_clt_load_init ;
        canFrame.byte.data.type1.cmd_data.load_sect_init.e = 1 ;
        canFrame.byte.data.type1.cmd_data.load_sect_init.s = 1 ;
        canFrame.byte.data.type1.cmd_data.load_sect_init.n = 4 - len ;
        canFrame.byte.data.type1.cmd_data.load_sect_init.x = 0 ;
        canFrame.byte.data.type1.m_index = htons(index.m_index) ;
        canFrame.byte.data.type1.m_subindex = index.m_subindex ;
        for(i = 0 ;i < len ;i ++)
        {
            canFrame.byte.data.type1.data.data8[i] = data[i];
        }
        ret = can_send(&canFrame);
    }
    else
    {

    }
	return ret ;
}

static int can_send(ucan_frame *canFrame)
{
	uint8 port = 0;
    CanMessage canMsg ;
    canMsg.m_canID = canFrame->byte.m_cob_id.m_CAN_ID;
    canMsg.m_dataLen = canFrame->byte.m_length;
    canMsg.m_extend = 1 ;
    canMsg.m_remote = 0 ;
    if(canFrame->byte.m_length <= 8)
    {
        memcpy(canMsg.m_data,canFrame->byte.data.m_data,canFrame->byte.m_length);
    }
    return can_send_msg(port ,&canMsg);
}


/**
 * @brief 任务末端要清除程序过程命令
 * 
 */
static void clean_cmd(void)
{
    can_run_msg.m_stop_cmd = 0 ;
    can_run_msg.m_ser_cmd  = 0 ;
    can_run_msg.m_ack_state = 0 ;  /*记录响应*/
}

/**
 * @brief 数据保存回调处理
 */
static int8_t sdo_write(uint8_t port, sindex m_index,int8_t *data,int16_t len)
{
    int8_t ret = ACK_ERR ;
    int16_t i ,j;
    sComponent_OBJ *p = NULL;
    
    if(port >= e_comp_max)
    {
        return ret ;
    }
    p = &comm_data_map_obj[port] ;
    for(i = 0;i<SELECT_SDO_OBJ_LEN(port);i++)
    {
        if(p->sdo_obj[i].index == m_index.index && p->sdo_obj[i].subindex == m_index.subindex )
        {
            if(len <= p->sdo_obj[i].data_len)
            {
                for(j = 0 ;j<len;j++)
                {
                    p->sdo_obj[i].ptr_data[j] = data[j];
                }
                if(NULL != p->sdo_obj[i].w_callback)
                {
                    p->sdo_obj[i].w_callback(p->sdo_obj[i].ptr_data);
                }
                ret = ACK ;
                return ret ;
            }
            else
            {
                can_run_msg.m_err_code = e_data_len_err ;
                return ret ;
            }
        }
    }
    if(i == SELECT_SDO_OBJ_LEN(port))
    {
        // TRACE_ERR("can not find write index\n");
        can_run_msg.m_err_code = e_obj_invalid_err ;
    }
    return ret ;
}

static int8_t sdo_read(uint8_t port, sindex m_index,int8_t *data,int16_t *len)
{
    int8_t ret = ACK_ERR ;
    int16_t i ,j ;
    sComponent_OBJ *p = NULL;
    
    if(port >= e_comp_max)
    {
        return ret ;
    }
    p = &comm_data_map_obj[port] ;
    for(i = 0;i<SELECT_SDO_OBJ_LEN(port);i++)
    {
        if(p->sdo_obj[i].index == m_index.index && p->sdo_obj[i].subindex == m_index.subindex)
        {
            for(j = 0 ;j<p->sdo_obj[i].data_len;j++)
            {
                data[j] = p->sdo_obj[i].ptr_data[j];
            }
            *len = p->sdo_obj[i].data_len;
            ret = ACK ;
            return ret ;
        }
    }
    return ret ;
}

static int8_t as_ser_wait_ack(void)
{
    
}


/************************************************** 客服端 **************************************************/
/**
 * @brief 作为客服端段下载启动协议
 * 
 * @param da_id ：  目标地址
 * @param m_index ：索引
 * @param is_fast ：快速传输
 * @param data ：   传输数据
 * @param len ：    数据长度
 * @return int8_t 
 */
static int8_t as_clt_segment_load_init(int8_t da_id,sindex m_index,uint8_t is_fast,int8_t *data,int8_t len)
{
    ucan_frame can_msg;
    int8_t ret = 0 ;
    int8_t i= 0 ;
    
    can_msg.byte.m_cob_id.byte.m_DA_ID = da_id ;
    can_msg.byte.m_cob_id.byte.m_SA_ID = CUR_MASTER_ID ;
    can_msg.byte.m_cob_id.byte.m_FUN_ID = SDO_RX_ID ;
    can_msg.byte.m_cob_id.byte.m_Reserver = 0 ;
    can_msg.byte.m_length = 8 ;
    can_msg.byte.data.type1.m_index = htons(m_index.index);
    can_msg.byte.data.type1.m_subindex = m_index.subindex ;
    if(is_fast)/*快速传输模式*/
    {
        if(len > 4)/*数据长度不可大于4*/
        {
            ret = -1 ;
            return ret ;
        }
        can_msg.byte.data.type1.cmd_data.load_sect_init.ccs = e_clt_load_init ;
        can_msg.byte.data.type1.cmd_data.load_sect_init.x = 0 ;
        can_msg.byte.data.type1.cmd_data.load_sect_init.n = 4 - len ;
        can_msg.byte.data.type1.cmd_data.load_sect_init.e = 1 ;
        can_msg.byte.data.type1.cmd_data.load_sect_init.s = 1 ;
        for(i = 0 ;i < len; i++)
        {
            can_msg.byte.data.type1.data.data8[i] = data[i];
        }
    }
    else
    {
        can_msg.byte.data.type1.cmd_data.load_sect_init.ccs = e_clt_load_init ;
        can_msg.byte.data.type1.cmd_data.load_sect_init.x = 0 ;
        can_msg.byte.data.type1.cmd_data.load_sect_init.n = 0 ;
        can_msg.byte.data.type1.cmd_data.load_sect_init.e = 0 ;
        can_msg.byte.data.type1.cmd_data.load_sect_init.s = 0 ;
        can_msg.byte.data.type1.data.data32 = 0 ;
    }
    ret = can_send(&can_msg);
    return ret ;
}

/**
 * @brief 作为客户端时：SDO段下载协议
 */
static int8_t as_clt_segment_load(int8_t da_id,sindex m_index,int8_t *data,int8_t len)
{
    ucan_frame can_msg;
    int8_t ret = 0 ;
    int8_t i= 0 ; 
    static uint8 count = 0;     // T标志
    static uint16  send_cnt = 0;

    can_msg.byte.m_cob_id.byte.m_DA_ID = da_id ;
    can_msg.byte.m_cob_id.byte.m_SA_ID = CUR_MASTER_ID ;
    can_msg.byte.m_cob_id.byte.m_FUN_ID = SDO_RX_ID ;
    can_msg.byte.m_cob_id.byte.m_Reserver = 0 ;

    if(len <= 4)
    {
        as_clt_segment_load_init(da_id, m_index, 1, data, len );
        com_data.ticks = 0;
        while(0 == com_data.m_ack_state)
        {
            if(com_data.ticks > 100)
                return CAN_RC_TIMEOUT;
        }
    }
    else
    {
        as_clt_segment_load_init(da_id, m_index, 0, data, len );

        /* 获取分段数 */
        com_data.remain_nums = len % 7;
        if( com_data.remain_nums == 0)
        {
            com_data.segment_nums = len / 7 ;
        }
        else
        {
            com_data.segment_nums = 1 + (len / 7) ;
        } 
        
        com_data.ticks = 0;
        while(com_data.segment_nums)
        {
            while(com_data.m_ack_state == 1)
            {
                com_data.ticks = 0;

                if(1 == com_data.segment_nums)
                {
                    /* 最后一段标志 */
                    can_msg.byte.data.type2.cmd_data.up_sect_ack.c = 1 ;

                    /* 最后一段数据长度 */
                    if(com_data.remain_nums != 0)
                        can_msg.byte.m_length =  com_data.remain_nums + 1 ;

                    if(com_data.remain_nums == 0)
                    {
                        can_msg.byte.data.type2.cmd_data.up_sect_ack.n = com_data.remain_nums ;
                    }
                    else
                    {
                        can_msg.byte.data.type2.cmd_data.up_sect_ack.n = 7 - com_data.remain_nums ;
                    } 
                }
                else
                {
                    can_msg.byte.m_length = 0x08 ;
                    can_msg.byte.data.type2.cmd_data.up_sect_ack.c = 0 ;
                    can_msg.byte.data.type2.cmd_data.up_sect_ack.n = 0 ;
                }

                can_msg.byte.data.type2.cmd_data.load_sect.ccs = e_clt_load ;
                can_msg.byte.data.type2.cmd_data.up_sect_ack.t = count % 2 ;
                count++ ;
                for(i = 0 ;i < can_msg.byte.m_length; i++)
                {
                    can_msg.byte.data.type2.m_data[i] = data[send_cnt++];
                }


                can_send(&can_msg);
                com_data.m_ack_state = 0;
                com_data.segment_nums -- ;

                
            }

            if(com_data.ticks > 100)
                return CAN_RC_TIMEOUT;
        }
    }
    return ret;
}


/**
 * @brief 作为客户端时：SDO段上传协议
 */
static int8_t as_clt_segment_up_requests(int8_t da_id,sindex m_index)
{
    ucan_frame can_msg;
    int8_t ret = 0 ;

    can_msg.byte.m_cob_id.byte.m_DA_ID = da_id ;
    can_msg.byte.m_cob_id.byte.m_SA_ID = CUR_MASTER_ID ;
    can_msg.byte.m_cob_id.byte.m_FUN_ID = SDO_RX_ID ;
    can_msg.byte.m_cob_id.byte.m_Reserver = 0 ;

    can_msg.byte.data.type1.cmd_data.up_sect_init.ccs = 0x2;
    can_msg.byte.data.type1.cmd_data.up_sect_init.x = 0;

    can_msg.byte.data.type1.m_index = htons(m_index.index);
    can_msg.byte.data.type1.m_subindex = m_index.subindex ; 

    can_msg.byte.data.type1.data.data32 = 0;
    can_msg.byte.m_length = 0x08 ;

    ret = can_send(&can_msg);  
    return ret;
}

/**
 * @brief 块传输初始化
 * 
 * @param da_id 
 * @param m_index 
 * @param len 
 * @return int8_t 
 */
static int8_t block_send_init(int8_t da_id,sindex m_index,int32_t len)
{
    ucan_frame can_msg;
    int8_t ret = 0 ;
    
    can_msg.byte.m_cob_id.byte.m_DA_ID = da_id ;
    can_msg.byte.m_cob_id.byte.m_SA_ID = CUR_MASTER_ID ;
    can_msg.byte.m_cob_id.byte.m_FUN_ID = SDO_RX_ID ;
    can_msg.byte.m_cob_id.byte.m_Reserver = 0 ;
    can_msg.byte.m_length = 8 ;
    can_msg.byte.data.type1.cmd_data.load_block_init.ccs = e_clt_blk_load ;
    can_msg.byte.data.type1.cmd_data.load_block_init.cc = 1 ;
    can_msg.byte.data.type1.cmd_data.load_block_init.cs = 0 ;
    can_msg.byte.data.type1.cmd_data.load_block_init.s = 1 ;
    can_msg.byte.data.type1.cmd_data.load_block_init.x = 0 ;
    can_msg.byte.data.type1.m_index = m_index.index ;
    can_msg.byte.data.type1.m_subindex = m_index.subindex ;
    can_msg.byte.data.type1.data.data32 = len ;
    can_run_msg.m_clt_cmd = e_blk_load_init ;
    can_run_msg.m_blk_load_result = false ;
    ret = can_send(&can_msg);
    return ret ;
}
/**
 * @brief 子块传输，数据长度应该是7的倍数，若不是，则表示是最后一块子块数据传输
 * 
 * @param da_id 
 * @param data 
 * @param len 
 * @return int8_t 
 */
static int8_t block_child_send(int8_t da_id,int8_t *data,uint8_t len)
{
    ucan_frame can_msg;
    int8_t ret = 1 ;
    uint8_t k = 0 ;
    int i,j;
    uint8_t frame_nums = len/7 ;

    can_msg.byte.m_cob_id.byte.m_DA_ID = da_id ;
    can_msg.byte.m_cob_id.byte.m_SA_ID = CUR_MASTER_ID ;
    can_msg.byte.m_cob_id.byte.m_FUN_ID = SDO_RX_ID ;
    can_msg.byte.m_cob_id.byte.m_Reserver = 0 ;
    can_msg.byte.m_length = 8 ;
    can_msg.byte.data.type2.cmd_data.load_sub_block.c = 0 ;
    for( i = 0 ;i < frame_nums;i++)
    {
        can_msg.byte.data.type2.cmd_data.load_sub_block.seqno = i ;
        for( j = 0;j < 7;j++)
        {
            can_msg.byte.data.type2.m_data[j] = data[k++] ;
        }
        if(1 != can_send(&can_msg))
        {
            ret = 0 ;
            break ;
        }
    }
    if(i == frame_nums)
    {
        frame_nums = len%7 ;
        can_msg.byte.m_length = frame_nums ;
        can_msg.byte.data.type2.cmd_data.load_sub_block.c = 1 ;/*最后一块*/
        can_msg.byte.data.type2.cmd_data.load_sub_block.seqno = frame_nums + 1 ;
        for(j = 0 ;j < frame_nums;j++)
        {
            can_msg.byte.data.type2.m_data[j] = data[k++] ;
        }
        if(1 != can_send(&can_msg))
        {
            ret = 0 ;
        }
    }
    can_run_msg.m_clt_cmd = e_blk_load_child ;
    can_run_msg.m_blk_load_result = false ;
    return ret ;
}
/**
 * @brief 块传输结束命令
 * 
 * @param da_id        
 * @param crc_data     
 * @param crc_check 是否进行CRC检验
 * @return int8_t      
 */ 
static int8_t block_send_end(int8_t da_id,int16_t crc_data,bool crc_check)
{
    int8_t ret = 0 ;
    ucan_frame can_msg;

    can_msg.byte.m_cob_id.byte.m_DA_ID = da_id ;
    can_msg.byte.m_cob_id.byte.m_SA_ID = CUR_MASTER_ID ;
    can_msg.byte.m_cob_id.byte.m_FUN_ID = SDO_RX_ID ;
    can_msg.byte.m_cob_id.byte.m_Reserver = 0 ;
    can_msg.byte.m_length = 8 ;
    can_msg.byte.data.type1.cmd_data.load_block_end.ccs = e_clt_blk_load ;
    can_msg.byte.data.type1.cmd_data.load_block_end.cs = 1 ;
    can_msg.byte.data.type1.cmd_data.load_block_end.n = 0 ;
    can_msg.byte.data.type1.cmd_data.load_block_end.x = 0 ;
    if(crc_check)
    {
        can_msg.byte.data.type1.m_index = crc_data ;
    }
    else
    {
        can_msg.byte.data.type1.m_index = 0 ;
    }
    ret = can_send(&can_msg);
    can_run_msg.m_clt_cmd = e_blk_load_end ;
    can_run_msg.m_blk_load_result = false ;
    return ret ;
}
/**
 * @brief 块上传输初始化
 * 
 * @param da_id 
 * @param m_index 
 * @param blk_size 
 * @return int8_t 
 */ 
static int8_t block_up_init(int8_t da_id,sindex m_index,uint8_t blk_size)
{
    int8_t ret = 0 ;
    ucan_frame can_msg;

    can_msg.byte.m_cob_id.byte.m_DA_ID = da_id ;
    can_msg.byte.m_cob_id.byte.m_SA_ID = CUR_MASTER_ID ;
    can_msg.byte.m_cob_id.byte.m_FUN_ID = SDO_RX_ID ;
    can_msg.byte.m_cob_id.byte.m_Reserver = 0 ;
    can_msg.byte.m_length = 8 ;
    can_msg.byte.data.type1.m_index = m_index.index ;
    can_msg.byte.data.type1.m_subindex = m_index.subindex ;
    can_msg.byte.data.type1.cmd_data.up_block_init1.ccs = e_clt_blk_up ;
    can_msg.byte.data.type1.cmd_data.up_block_init1.cc = 1 ;
    can_msg.byte.data.type1.cmd_data.up_block_init1.x = 0 ;
    can_msg.byte.data.type1.cmd_data.up_block_init1.cs = 0 ;
    can_msg.byte.data.type1.data.data8[0] = blk_size ;
    can_msg.byte.data.type1.data.data8[1] = 0 ;
    can_msg.byte.data.type1.data.data8[2] = 0 ;
    can_msg.byte.data.type1.data.data8[3] = 0 ;

    ret = can_send(&can_msg);
    return ret ;
}


/**
 * @brief 作为客服端时SDO命令
 */
int8_t clt_sdo_com_test(void)
{
    sindex ind;
    int8_t ret;
	static bool only = 1;
    uint8 send_data[4] = {0xA1, 0xA2, 0xA3, 0xA4};
    uint8 send_data1[20];
    uint8 i;

    for(i = 0; i < 20; i++)
    {
        send_data1[i] = i * 2;
    }
    
    ind.index = 0x2000;
    ind.subindex = 0x06;

	if(only == 1)
	{
		only = 0;
        as_clt_segment_up_requests(0x01, ind);
		// ret = as_clt_segment_load_init(0x01, ind, 1,send_data, 4);
	}
		
    // while(com_data.m_ack_state == 1)
    // {
    //     com_data.m_ack_state = 0;
    //     as_clt_segment_load_init(0x01, ind, 1,send_data, 4);
    // }

    //as_clt_segment_load(0x01, ind, send_data1, sizeof(send_data1));
    // as_clt_segment_up(0x01, ind);

}
/************************************************** 客服端 **************************************************/

/**
 * @brief 作为客户端：段下载初始化 ACK
 */ 
static int8_t as_ser_segment_load_init_ack(uint8_t port, ucan_frame *can_msg)
{
    com_data.m_ack_state = 1 ;  /*记录ACK响应*/
}

/**
 * @brief 作为客户端：分段段下载 ACK
 */ 
static int8_t as_ser_segment_load_ack(uint8_t port, ucan_frame *can_msg)
{
    com_data.m_ack_state = 1 ;  /*记录ACK响应*/
    if(1 == can_msg->byte.data.type1.cmd_data.load_sect_ack.x)/*分段发送完成*/
    { 
        // t[port] = 0 ;
    } 
}

/**
 * @brief 作为客户端：段上传初始化 ACK
 */ 
static int8_t as_ser_segment_up_init_ack(uint8_t port, ucan_frame *can_msg)
{
	uint32 i1, i2;
    int8_t ret = 0 ;
    int16_t temp;
    int8_t i, j;
    sindex t_index;
    ucan_frame send_msg = {0};
    sComponent_OBJ *p = &comm_data_map_obj[port];

    /*快传*/
    if(1 == can_msg->byte.data.type1.cmd_data.up_sect_init_ack.e &&
        1 == can_msg->byte.data.type1.cmd_data.up_sect_init_ack.s )
    {
        sComponent_OBJ *p = &comm_data_map_obj[port];
        for(i = 0 ;i < SELECT_SDO_OBJ_LEN(port);i++)
        {
            if(p->sdo_obj[i].index == htons(can_msg->byte.data.type1.m_index) && 
                p->sdo_obj[i].subindex == can_msg->byte.data.type1.m_subindex)
            {
                temp = 4 - can_msg->byte.data.type1.cmd_data.up_sect_init_ack.n;
                if(temp == p->sdo_obj[i].data_len)
                {
                    for(j = 0 ;j < temp;j++)
                    {
                            p->sdo_obj[i].ptr_data[j] = can_msg->byte.data.type1.data.data8[j];
                    }
                    if(NULL != p->sdo_obj[i].r_callback)
                    {
                        p->sdo_obj[i].r_callback((void *)p->sdo_obj[i].ptr_data);
                    }
                }
                else /* size err */
                {
                    t_index.index = htons(can_msg->byte.data.type1.m_index) ;
                    t_index.subindex = can_msg->byte.data.type1.m_subindex ;
                    send_err(SDO_RX_ID,can_msg->byte.m_cob_id.byte.m_SA_ID,t_index,e_data_len_err);
                }
                break;
            }
            else
            {
            }
        }
    }
    else /* 分段传输 */
    {

        com_data.state.use_pre_state = 1;
        com_data.state.pre_state = e_ser_up;
        com_data.state.m_index.index = htons(can_msg->byte.data.type1.m_index );
        com_data.state.m_index.subindex = can_msg->byte.data.type1.m_subindex ;

        /*分段上传*/
        send_msg.byte.m_cob_id.byte.m_DA_ID = can_msg->byte.m_cob_id.byte.m_SA_ID ;
        send_msg.byte.m_cob_id.byte.m_SA_ID = CUR_MASTER_ID ;
        send_msg.byte.m_cob_id.byte.m_FUN_ID = SDO_RX_ID ;
        send_msg.byte.m_length = SDO_FRAME_LEN ;
        send_msg.byte.data.type2.cmd_data.up_sect.ccs = e_clt_up ;
        send_msg.byte.data.type2.cmd_data.up_sect.t = 0 ;
        send_msg.byte.data.type2.cmd_data.up_sect.x = 0 ;
        for(i =0 ;i<7;i++)
        {
            send_msg.byte.data.type2.m_data[i] = 0;
        }
        if(0 == can_send(&send_msg))
        {
            //TRACE_ERR("can send frame fail\n");
            ret = CAN_RC_ERROR ;
        }
    }
	return ret;
}

/**
 * @brief 作为客户端：分段上传 ACK
 */ 
static int8_t as_ser_segment_up_ack(uint8_t port, ucan_frame *can_msg)
{
    int8_t ret = 0 ;
    int16_t temp;
    int8_t i, j;
    static int16_t t[e_comp_max] ;

    if(e_ser_up == can_msg->byte.data.type2.cmd_data.up_sect_ack.scs)
    {
        if(can_msg->byte.data.type2.cmd_data.up_sect_ack.t == t[port]%2)
        {
            t[port]++;
            /* 最后一段 */
            if(1 == can_msg->byte.data.type2.cmd_data.up_sect_ack.c)
            {
                temp = 7-can_msg->byte.data.type2.cmd_data.up_sect_ack.n ;
                for(j = 0 ;j < temp;j++)
                {
                    com_data.sdo_buff[com_data.sdo_index++] = can_msg->byte.data.type2.m_data[j];
                }
                if(ACK!=sdo_write(port, com_data.state.m_index, com_data.sdo_buff, com_data.sdo_index))
                {
                    ret = CAN_RC_ERROR ;
                }

                t[port] = 0  ;
                com_data.state.use_pre_state = 0;
                com_data.state.pre_state = 0;
                com_data.state.m_index.index = 0;
                com_data.state.m_index.subindex = 0 ;  
                /* 复位buf */
                memset(com_data.sdo_buff, 0 , com_data.sdo_index);
                com_data.sdo_index = 0;             
                // pthread_cond_signal(&g_cond[port]); /*触发信号量*/
            }
            else
            {
                for(j = 0 ;j < 7;j++)
                {
                    com_data.sdo_buff[com_data.sdo_index++] = can_msg->byte.data.type2.m_data[j];
                }
            }
        }
        else
        {
            t[port] = 0  ;
            com_data.state.use_pre_state = 0;
            com_data.state.pre_state = 0;
            com_data.state.m_index.index = 0;
            com_data.state.m_index.subindex = 0 ;  
            /* 复位buf */
            memset(com_data.sdo_buff, 0 , com_data.sdo_index);
            com_data.sdo_index = 0;   

            can_run_msg.m_stop_cmd = 1 ;
            can_run_msg.m_err_code = e_rolling_over_err ;
            /*状态机去做处理*/
            ret = ACK ;
        }
    }
    else
    {
        ret = CAN_RC_ID_IS_OUT ;
    }
	return ret;
}
/**
 * @brief 作为客户端时响应服务器命令
 * 
 * @param port :can端口
 * @param can_msg can数据帧
 * @return int8_t 返回状态
 */ 
static int8_t clt_sdo_process(uint8_t port, ucan_frame *can_msg)
{
    int8_t ret = CAN_RC_OK ;
    int16_t i,j,temp,index;
    sindex t_index ;
    ucan_frame send_msg ;
    static int8_t pre_state[e_comp_max] ;
    static int8_t port_log[e_comp_max] ;
    static int16_t has_rec_len[e_comp_max];
    static int8_t state = 0 ;
    static sindex m_index[e_comp_max] ;
    static int16_t t[e_comp_max] ;

    sComponent_OBJ *p = &comm_data_map_obj[port];
    // if(port_log[port])
    // {
    //     state = pre_state[port];
    // }
    // else
    // {
    //     state = can_msg->byte.data.type1.cmd_data.load_sect_ack.scs ;
    // }  
       
    if(com_data.state.use_pre_state == 1)
    {
        state = com_data.state.pre_state;
    }
    else
    {
        state = can_msg->byte.data.type1.cmd_data.load_sect_init.ccs;
    } 

    switch(state)
    {
        case e_ser_load_init:           /* 段下载初始化 */
            as_ser_segment_load_init_ack(port, can_msg);
        break;
            
        case e_ser_load:                /* 分段下载 */
            as_ser_segment_load_ack(port, can_msg);
        break;

        case e_ser_up_init:             /* 段上传初始化 */
            as_ser_segment_up_init_ack(port, can_msg);
        break;
        
        case e_ser_up:                  /* 段上传 */
            as_ser_segment_up_ack(port, can_msg);
        break;
        
        case e_ser_stop_cmd:
            can_run_msg.m_stop_cmd = 1 ;
            can_run_msg.m_err_code = htonl(can_msg->byte.data.type1.data.data32) ;
            t[port] = 0 ;
            port_log[port] = 0 ;
            ret = ACK ;
        break;
        case e_ser_blk_load:
            // if(e_blk_load_init == can_run_msg.m_clt_cmd)
            // {
            //     can_run_msg.m_blk_load_size = can_msg->byte.data.m_data[4];
            //     can_run_msg.m_blk_load_result = true ;
            // }
            // else if(e_blk_load_child == can_run_msg.m_clt_cmd)
            // {
            //     if(2 == can_msg->byte.data.type1.cmd_data.load_sub_block_ack.ss && \
            //       can_msg->byte.data.m_data[1] == can_msg->byte.data.m_data[2])
            //     {
            //         can_run_msg.m_blk_load_ack_size = can_msg->byte.data.m_data[2];
            //         can_run_msg.m_blk_load_result = true ;
            //     }
            //     else
            //     {
            //         can_run_msg.m_blk_load_result = false ;
            //     }
            // }
            // else if(e_blk_load_end == can_run_msg.m_clt_cmd)
            // {
            //     if(1 == can_msg->byte.data.type1.cmd_data.load_block_end_ack.ss)
            //     {
            //         can_run_msg.m_blk_load_result = true ;
            //     }
            //     else
            //     {
            //         can_run_msg.m_blk_load_result = false ;
            //     }
            // }
        break;
        case e_ser_blk_up:
            
        break;
        default :
            
        break;
    }

    return ret ;
}

/************************************************** 服务器 **************************************************/

/**
 * @brief 作为服务器时：SDO段下载启动协议
 */
static int8_t as_ser_segment_load_init(uint8_t port, ucan_frame *can_msg)
{
    ucan_frame send_msg;
    int8_t i, j;
    int16_t temp;
    sindex t_index;
    sComponent_OBJ *p = &comm_data_map_obj[port];

    /* SDO回复数据头 */
    send_msg.byte.m_cob_id.byte.m_SA_ID = CUR_MASTER_ID;
    send_msg.byte.m_cob_id.byte.m_DA_ID = can_msg->byte.m_cob_id.byte.m_SA_ID;
    send_msg.byte.m_cob_id.byte.m_FUN_ID = SDO_TX_ID;
    send_msg.byte.m_cob_id.byte.m_Reserver = 0 ;
    send_msg.byte.m_length = 0x08 ;
    send_msg.byte.data.type1.m_index = can_msg->byte.data.type1.m_index ;
    send_msg.byte.data.type1.m_subindex = can_msg->byte.data.type1.m_subindex ;

    /* 带字节大小快速下载：多一次数据长度校验*/
    if( 1 == can_msg->byte.data.type1.cmd_data.load_sect_init.e &&
        1 == can_msg->byte.data.type1.cmd_data.load_sect_init.s)
    {
        for(i = 0 ;i<SELECT_SDO_OBJ_LEN(port);i++)
        {
            if( p->sdo_obj[i].index == htons(can_msg->byte.data.type1.m_index) &&
                p->sdo_obj[i].subindex == can_msg->byte.data.type1.m_subindex)
            {
                temp = 4 - can_msg->byte.data.type1.cmd_data.load_sect_init.n;
                if(p->sdo_obj[i].data_len == temp)
                {
                    for(j = 0 ;j < temp; j++)
                    {
                        p->sdo_obj[i].ptr_data[j] = can_msg->byte.data.type1.data.data8[j];
                    }
                    if(NULL != p->sdo_obj[i].w_callback)
                    {
                        p->sdo_obj[i].w_callback((void *)p->sdo_obj[i].ptr_data);
                    }

                    /* SDO服务器相应 */
                    send_msg.byte.data.type1.cmd_data.load_sect_init_ack.scs = e_ser_load_init;
                    send_msg.byte.data.type1.cmd_data.load_sect_init_ack.x = 0;
                    can_send(&send_msg);
                }
                else
                {
                    /* 长度异常 */
                    t_index.index = htons(can_msg->byte.data.type1.m_index) ;
                    t_index.subindex = can_msg->byte.data.type1.m_subindex ;
                    send_err(SDO_TX_ID,can_msg->byte.m_cob_id.byte.m_SA_ID,t_index,e_data_len_err);
                }
                break;
            }
        }
    }
    /* 不带字节大小快速下载 */
    else if(1 == can_msg->byte.data.type1.cmd_data.load_sect_init.e &&
            0 == can_msg->byte.data.type1.cmd_data.load_sect_init.s)
    {
        for(i = 0 ;i<SELECT_SDO_OBJ_LEN(port);i++)
        {
            if( p->sdo_obj[i].index == htons(can_msg->byte.data.type1.m_index) &&
                p->sdo_obj[i].subindex == can_msg->byte.data.type1.m_subindex)
            {
                if(p->sdo_obj[i].data_len <= 4)
                {
                    for(j = 0 ;j < p->sdo_obj[i].data_len;j++)
                    {
                        p->sdo_obj[i].ptr_data[j] = can_msg->byte.data.type1.data.data8[j];
                    }
                    if(NULL != p->sdo_obj[i].w_callback)
                    {
                        p->sdo_obj[i].w_callback((void *)p->sdo_obj[i].ptr_data);
                    }

                    /* SDO服务器相应 */
                    send_msg.byte.data.type1.cmd_data.load_sect_init_ack.scs = e_ser_load_init;
                    send_msg.byte.data.type1.cmd_data.load_sect_init_ack.x = 0;
                    can_send(&send_msg);
                }
                else
                {
                    /* 数据长度异常 */
                    t_index.index = htons(can_msg->byte.data.type1.m_index) ;
                    t_index.subindex = can_msg->byte.data.type1.m_subindex ;
                    send_err(SDO_TX_ID,can_msg->byte.m_cob_id.byte.m_SA_ID,t_index,e_data_len_err);
                }
                break;
            }
        }
    }
    else
    {
        /* TODO:记录分段接收的索引，用于分段接收索引地址*/
        com_data.state.m_index.index =  htons(can_msg->byte.data.type1.m_index) ;
        com_data.state.m_index.subindex = can_msg->byte.data.type1.m_subindex ;

        /* TODO:记录下一次状态机要进入的状态*/
        com_data.state.later_state = e_clt_load;

        /* SDO服务器相应 */
        send_msg.byte.data.type1.cmd_data.load_sect_init_ack.scs = e_ser_load_init;
        send_msg.byte.data.type1.cmd_data.load_sect_init_ack.x = 0;
        can_send(&send_msg);
    }
}

/**
 * @brief 作为服务器时：SDO分段下载协议
 */ 
static int8_t as_ser_segment_load(uint8_t port, ucan_frame *can_msg)
{
	int8_t ret = 0 ;
    int16_t temp;
    int8_t i, j;
    sindex t_index;
    ucan_frame send_msg = {0};
	static int16_t t[e_comp_max];
	
    /* SDO回复数据头 */
    send_msg.byte.m_cob_id.byte.m_SA_ID = CUR_MASTER_ID;
    send_msg.byte.m_cob_id.byte.m_DA_ID = can_msg->byte.m_cob_id.byte.m_SA_ID;
    send_msg.byte.m_cob_id.byte.m_FUN_ID = SDO_TX_ID;
    send_msg.byte.m_cob_id.byte.m_Reserver = 0 ;
    send_msg.byte.m_length = 0x08 ;
    send_msg.byte.data.type1.m_index = can_msg->byte.data.type1.m_index ;
    send_msg.byte.data.type1.m_subindex = can_msg->byte.data.type1.m_subindex ;

    t_index.index = com_data.state.m_index.index;
    t_index.subindex = com_data.state.m_index.subindex ;

    if(e_clt_load == can_msg->byte.data.type1.cmd_data.load_sect.ccs)/*判断命令码*/
    {
        if(can_msg->byte.data.type1.cmd_data.load_sect.c) /*1:最后一段*/
        {
            temp = 7 - can_msg->byte.data.type1.cmd_data.load_sect.n;
            for(j = 0 ;j < temp;j++)
            {
                com_data.sdo_buff[com_data.sdo_index++] = can_msg->byte.data.type2.m_data[j];
            }
            /* 回调处理 */
            if(ACK != sdo_write(port, com_data.state.m_index,  com_data.sdo_buff, com_data.sdo_index))
            {
                send_err(SDO_TX_ID, can_msg->byte.m_cob_id.byte.m_SA_ID, t_index, e_obj_invalid_err);
            }
            else
            {
                t[port]++ ;
                send_msg.byte.data.type2.cmd_data.load_sect_ack.scs = e_ser_load ;
                send_msg.byte.data.type2.cmd_data.load_sect_ack.t = t[port]%2;
                send_msg.byte.data.type2.cmd_data.load_sect_ack.x = 1 ;
                if(0 == can_send(&send_msg))
                {
                    ret = CAN_RC_ERROR;
                }
            }
//            port_log[port] = 0 ;  /*清楚标志*/
//            pre_state[port] = 0 ;
            t[port] = 0  ;

            memset(&com_data.sdo_buff, 0 , com_data.sdo_index);
            com_data.sdo_index = 0 ;
            com_data.state.m_index.index = 0;
            com_data.state.m_index.subindex = 0;
        }
        else
        {
            if(can_msg->byte.data.type1.cmd_data.load_sect.t == (t[port]%2))
            {
                for(j = 0 ;j < 7;j++)
                {
                    com_data.sdo_buff[com_data.sdo_index++] = can_msg->byte.data.type2.m_data[j];
                }
                t[port]++ ;
                send_msg.byte.data.type2.cmd_data.load_sect_ack.scs = e_ser_load ;
                send_msg.byte.data.type2.cmd_data.load_sect_ack.t = t[port]%2;
                send_msg.byte.data.type2.cmd_data.load_sect_ack.x = 0 ;
                can_send(&send_msg);/*过程不响应，响应最后一阵即可*/
            }
            else/*t没反转错误*/
            {
                send_err(SDO_TX_ID, can_msg->byte.m_cob_id.byte.m_SA_ID, t_index, e_rolling_over_err);
//                port_log[port] = 0 ;  /*清楚标志*/
//                pre_state[port] = 0 ;
                t[port] = 0  ;

                memset(&com_data.sdo_buff, 0 , com_data.sdo_index);
                com_data.sdo_index = 0 ;
                com_data.state.m_index.index = 0;
                com_data.state.m_index.subindex = 0;
            }
        }
        
    }
    else/*指令错误*/
    {
        send_err(SDO_TX_ID,can_msg->byte.m_cob_id.byte.m_SA_ID,t_index,e_ccs_cmd_err);
    }    
}

/**
 * @brief 作为服务器时：SDO块下载协议
 */ 
static int8_t as_ser_block_load(uint8_t port, ucan_frame *can_msg)
{
	uint32 i1, i2;
    int8_t ret = 0 ;
    int16_t temp;
    int8_t i, j;
    sindex t_index;
    ucan_frame send_msg = {0};
    sComponent_OBJ *p = &comm_data_map_obj[port];
	
    /* SDO回复数据头 */
    send_msg.byte.m_cob_id.byte.m_SA_ID = CUR_MASTER_ID;
    send_msg.byte.m_cob_id.byte.m_DA_ID = can_msg->byte.m_cob_id.byte.m_SA_ID;
    send_msg.byte.m_cob_id.byte.m_FUN_ID = SDO_TX_ID;
    send_msg.byte.m_cob_id.byte.m_Reserver = 0 ;
    send_msg.byte.m_length = 0x08 ;
	send_msg.byte.data.m_data[0] = 0;
    send_msg.byte.data.m_data[1] = 0;
    send_msg.byte.data.m_data[2] = 0;
    send_msg.byte.data.m_data[3] = 0;
    send_msg.byte.data.m_data[4] = 0;
    send_msg.byte.data.m_data[5] = 0;
    send_msg.byte.data.m_data[6] = 0;
    send_msg.byte.data.m_data[7] = 0;

    /* 块初始化：保存块大小(字节) */
    /* 返回：子块数 */
    if(e_blk_load_init == com_data.state.blk_load_state)
    {

        /* 计算子块数 */
		com_data.sdo_blk_size = htonl(can_msg->byte.data.type1.data.data32);
        com_data.sdo_blk_num = htonl(can_msg->byte.data.type1.data.data32) / 512 + (htonl(can_msg->byte.data.type1.data.data32) % 512) ? 1 : 0;
		i1 = can_msg->byte.data.type1.data.data32;
		i2 = htonl(can_msg->byte.data.type1.data.data32);

        send_msg.byte.data.type1.cmd_data.load_block_init_ack.scs   = 5;
        send_msg.byte.data.type1.cmd_data.load_block_init_ack.x     = 0;
        send_msg.byte.data.type1.cmd_data.load_block_init_ack.sc    = 1;
        send_msg.byte.data.type1.cmd_data.load_block_init_ack.ss    = 0;
        send_msg.byte.data.type1.m_index = can_msg->byte.data.type1.m_index ;
        send_msg.byte.data.type1.m_subindex = can_msg->byte.data.type1.m_subindex ;
        send_msg.byte.data.type1.data.data32 = com_data.sdo_blk_num;
        ret = can_send(&send_msg); 

        //保存SDO对象
        for(i = 0 ;i<SELECT_SDO_OBJ_LEN(port);i++)
        {
            if(p->sdo_obj[i].index == htons(can_msg->byte.data.type1.m_index) 
            &&p->sdo_obj[i].subindex == can_msg->byte.data.type1.m_subindex)
            {
                com_data.state.sdo_p = &p->sdo_obj[i];
            }
        }

        /* 确定是否启动子块传输 */
        if(0 == can_msg->byte.data.type1.cmd_data.load_block_init.cs)
        {
            com_data.state.blk_load_state = e_blk_load_child;
        }
    }
    /* 子块传输： 每一个子块进行回调*/
    /* 子块结构：crc16 + 子块大小 + 保留2B + 数据n */
    else if(e_blk_load_child == com_data.state.blk_load_state)
    {
        if(0 == can_msg->byte.data.type2.cmd_data.load_sub_block.c)
        {
            uint8 n = can_msg->byte.data.type2.cmd_data.load_sub_block.seqno;
            for(i = 0; i < 7; i++)
            {
                com_data.blk_buff.sdo_blk_buff[com_data.sdo_blk_index++] = can_msg->byte.data.type2.m_data[i];
            }                 
        }
        else
        {
			com_data.sdo_blk_cnt++;
            /* 计算当前子块大小 */
            if( com_data.sdo_blk_cnt < com_data.sdo_blk_num )  com_data.sdo_blk_child_size = 512;
            else com_data.sdo_blk_child_size = com_data.sdo_blk_size % 512;

            // uint8 n = can_msg->byte.data.type2.cmd_data.load_sub_block.seqno;
            uint8 n = can_msg->byte.m_length;
            for(i = 0; i < n - 1; i++)
            {
                com_data.blk_buff.sdo_blk_buff[com_data.sdo_blk_index++] = can_msg->byte.data.type2.m_data[i];
            }
            if(com_data.sdo_blk_child_size + 6 == com_data.sdo_blk_index)
            {
                if(NULL != com_data.state.sdo_p->w_callback)
                {
                    ret = com_data.state.sdo_p->w_callback((void *)com_data.state.sdo_p->ptr_data);
                }
                send_msg.byte.data.type1.cmd_data.load_sub_block_ack.scs = 5;
                send_msg.byte.data.type1.cmd_data.load_sub_block_ack.x   = 0;
                send_msg.byte.data.type1.cmd_data.load_sub_block_ack.ss  = 2;
                send_msg.byte.data.type2.m_data[0]  =   ret;
                send_msg.byte.data.type2.m_data[1]  =   n;
                can_send(&send_msg);

                /* 复位buf */
                memset(com_data.blk_buff.sdo_blk_buff, 0, com_data.sdo_blk_index);
                com_data.sdo_blk_index = 0;

                /* 块结束判断 */
                if( com_data.sdo_blk_cnt == com_data.sdo_blk_num )
                    com_data.state.blk_load_state = e_blk_load_end;
            }
            else{
                t_index.index = htons(can_msg->byte.data.type1.m_index) ;
                t_index.subindex = can_msg->byte.data.type1.m_subindex ;
                send_err(SDO_TX_ID,can_msg->byte.m_cob_id.byte.m_SA_ID,t_index,e_data_len_err);
            }   
        }  
    }
    /* 块结束：校验 */
    else if(e_blk_load_end == com_data.state.blk_load_state)
    {
        if(can_msg->byte.data.type_crc.crc16[1] == 0)
            com_data.blkcrc.crc32 = htonl(can_msg->byte.data.type_crc.crc16[1] << 16) + htons(can_msg->byte.data.type_crc.crc16[0]);
        if(1 == can_msg->byte.data.type1.cmd_data.load_block_end.cs)
        {
            if(NULL != com_data.state.sdo_p->w_callback)
            {
                ret = com_data.state.sdo_p->w_callback((void *)com_data.state.sdo_p->ptr_data);
            }
        }
        send_msg.byte.data.type1.cmd_data.load_block_end_ack.scs =5; 
        send_msg.byte.data.type1.cmd_data.load_block_end_ack.x   =0; 
        send_msg.byte.data.type1.cmd_data.load_block_end_ack.sc  =ret; 
        send_msg.byte.data.type1.cmd_data.load_block_end_ack.ss  =1; 
        send_msg.byte.data.type2.m_data[0]  =   0;

        can_send(&send_msg);

        com_data.state.blk_load_state = e_blk_load_init;
    }   
}


/**
 * @brief 作为服务器时：SDO段上传启动协议
 */
static int8_t as_ser_segment_up_init(uint8_t port, ucan_frame *can_msg)
{
	int8_t ret = 0 ;
    ucan_frame send_msg;
    int8_t i, j;
    int16_t temp;
    sindex t_index;
    sComponent_OBJ *p = &comm_data_map_obj[port];

    /* SDO回复数据头 */
    send_msg.byte.m_cob_id.byte.m_SA_ID = CUR_MASTER_ID;
    send_msg.byte.m_cob_id.byte.m_DA_ID = can_msg->byte.m_cob_id.byte.m_SA_ID;
    send_msg.byte.m_cob_id.byte.m_FUN_ID = SDO_TX_ID;
    send_msg.byte.m_cob_id.byte.m_Reserver = 0 ;
    send_msg.byte.m_length = 0x08 ;
    send_msg.byte.data.type1.m_index = can_msg->byte.data.type1.m_index ;
    send_msg.byte.data.type1.m_subindex = can_msg->byte.data.type1.m_subindex ;

    if(e_clt_up_init == can_msg->byte.data.type1.cmd_data.load_sect.ccs)                        /*判断命令码*/
    {

        for(i = 0 ;i<SELECT_SDO_OBJ_LEN(port);i++)
        {
            if(p->sdo_obj[i].index == htons(can_msg->byte.data.type1.m_index) &&
                p->sdo_obj[i].subindex == can_msg->byte.data.type1.m_subindex)
            {
                /* 快传 */
                if(p->sdo_obj[i].data_len <= 4)
                {
                    if(NULL != p->sdo_obj[i].r_callback)
                    {
                        p->sdo_obj[i].r_callback((void *)p->sdo_obj[i].ptr_data);
                    }
                    for(j = 0 ;j < p->sdo_obj[i].data_len;j++)
                    {
                        send_msg.byte.data.type1.data.data8[j] = p->sdo_obj[i].ptr_data[j] ;   /*换成数据类型转换好的*/
                    }
                    
                    send_msg.byte.data.type1.cmd_data.up_sect_init_ack.scs = e_ser_up_init;
                    send_msg.byte.data.type1.cmd_data.up_sect_init_ack.x = 0;
                    send_msg.byte.data.type1.cmd_data.up_sect_init_ack.n = 4-p->sdo_obj[i].data_len;
                    send_msg.byte.data.type1.cmd_data.up_sect_init_ack.e = 1 ;
                    send_msg.byte.data.type1.cmd_data.up_sect_init_ack.s = 1 ;
                    can_send(&send_msg);
                }
                /* 分段传输 */
                else
                {
                    /*记录分段接收的索引，用于分段接收索引地址*/
                    com_data.state.m_index.index = htons(can_msg->byte.data.type1.m_index);
                    com_data.state.m_index.subindex = can_msg->byte.data.type1.m_subindex;

                    send_msg.byte.data.type1.cmd_data.up_sect_init_ack.scs = e_ser_up_init;
                    send_msg.byte.data.type1.cmd_data.up_sect_init_ack.x = 0;
                    send_msg.byte.data.type1.cmd_data.up_sect_init_ack.n = 0;
                    send_msg.byte.data.type1.cmd_data.up_sect_init_ack.e = 0 ;
                    send_msg.byte.data.type1.cmd_data.up_sect_init_ack.s = 0 ;
                    can_send(&send_msg);
                }
                break;
            }
        }
    }
    else
    {
        t_index.index = htons(can_msg->byte.data.type1.m_index) ;
        t_index.subindex = can_msg->byte.data.type1.m_subindex ;
        send_err(SDO_TX_ID,can_msg->byte.m_cob_id.byte.m_SA_ID,t_index,e_ccs_cmd_err);
    }
	
	return ret;
}

/**
 * @brief 段发送接口，linux下只管发送数据即可，无需关心队列满得问题,
 * 使用while循环一直发送直达发送完成
 * @param da_id 目标地址
 * @param data  数据
 * @param len   数据长度
 * @return int8_t  返回类型
 */
static int8_t segment_send(int8_t da_id,int8_t *data , uint16_t len)
{        
    int8_t ret = 0 ;
    ucan_frame can_msg;
    uint16_t seg_nums = 0 ;
    uint8_t remain_nums = 0 ;
    uint16_t count = 0 ;
    uint16_t send_cnt = 0 ;
    bool re_send_flag = false ;/*重新发送标志*/
    uint8_t i = 0 ;
    int failCnt = 0 ;
        
    remain_nums = len % 7 ;
    if(remain_nums == 0)
    {
        seg_nums = len / 7 ;
    }
    else
    {
        seg_nums = 1 + (len / 7) ;
    }
    can_msg.byte.m_cob_id.byte.m_DA_ID = da_id ;
    can_msg.byte.m_cob_id.byte.m_SA_ID = CUR_MASTER_ID ;
    can_msg.byte.m_cob_id.byte.m_FUN_ID = SDO_TX_ID ;
    can_msg.byte.m_cob_id.byte.m_Reserver = 0 ;
    
    while(seg_nums)
    {
        if(false == re_send_flag)
        {
            if(1 == seg_nums)
            {
                if(remain_nums != 0)
                    can_msg.byte.m_length =  len % 7 + 1 ;
                can_msg.byte.data.type2.cmd_data.up_sect_ack.c = 1 ;
                if(remain_nums == 0)
                {
                    can_msg.byte.data.type2.cmd_data.up_sect_ack.n = remain_nums ;
                }
                else
                {
                    can_msg.byte.data.type2.cmd_data.up_sect_ack.n = 7 - remain_nums ;
                } 
            }
            else
            {
                can_msg.byte.m_length = 0x08 ;
                can_msg.byte.data.type2.cmd_data.up_sect_ack.c = 0 ;
                can_msg.byte.data.type2.cmd_data.up_sect_ack.n = 0 ;
            }
            can_msg.byte.data.type2.cmd_data.up_sect_ack.scs = e_ser_up ;
            can_msg.byte.data.type2.cmd_data.up_sect_ack.t = count % 2 ;
            count++ ;
            for(i = 0 ;i < 7;i++)
            {
                can_msg.byte.data.type2.m_data[i] = data[send_cnt++];
            }
        }
        if(true == can_send(&can_msg))/*遇到缓存满得情况，则再一次发送*/
        {
            seg_nums -- ;
            re_send_flag = false ;
            if(0 == seg_nums)
            {
                ret = ACK ;
            }
        }
        else
        {
            re_send_flag = true ;
            failCnt++;
            if(failCnt > 500)//发送失败超过500次则退出发送
            {
                ret = CAN_RC_ERROR ;
                break;
            }
        } 
    }
    return ret;
}

/**
 * @brief 作为服务器时：SDO分段上传协议
 */ 
static int8_t as_ser_segment_up(uint8_t port, ucan_frame *can_msg)
{
    int16_t data_len = 0;
	int8_t i;
    sComponent_OBJ *p = &comm_data_map_obj[port];
    for(i = 0 ;i<SELECT_SDO_OBJ_LEN(port);i++)
    {
        if( p->sdo_obj[i].index == com_data.state.m_index.index &&
            p->sdo_obj[i].subindex ==  com_data.state.m_index.subindex)
        {
            com_data.sdo_index = 0 ;
            if(NULL != p->sdo_obj[i].r_callback)
            {
                p->sdo_obj[i].r_callback((void *)p->sdo_obj[i].ptr_data);
            }
            memcpy(com_data.sdo_buff, p->sdo_obj[i].ptr_data, p->sdo_obj[i].data_len);
            com_data.sdo_index = p->sdo_obj[i].data_len ;
        }
    }
    segment_send(can_msg->byte.m_cob_id.byte.m_SA_ID, com_data.sdo_buff, com_data.sdo_index);   
}

/**
 * @brief SDO终止协议
 * @return int8_t 
 */
int8_t sdo_stop(ucan_frame *can_msg)
{
    int8_t ret;

    com_data.m_stop_cmd = 1 ;
    com_data.state.m_index.index = can_msg->byte.data.type1.m_index;
    com_data.state.m_index.subindex = can_msg->byte.data.type1.m_subindex;
//    com_data.can_pkg = *can_msg ;
    ret = ACK ;
    return ret;
}

/**
 * @brief 作为服务器时响应客户端命令
 * 
 * @param port :can端口
 * @param can_msg can数据帧
 * @return int8_t 返回状态
 */
static int8_t ser_sdo_process(uint8_t port, ucan_frame *can_msg)
{
    int8_t ret = 0 ;
    static int8_t state = 0 ;

	/* 快传输确定 */
    if(com_data.state.blk_load_state != e_blk_load_init)
    {
        state = e_clt_blk_load;
    }
    else
    {
        state = can_msg->byte.data.type1.cmd_data.load_sect_init.ccs;
    }
    
    switch(state)
    {
        case e_clt_load:            /* 分段下载解析 */
			as_ser_segment_load(port, can_msg);
        break;

        case e_clt_load_init:       /* 段下载启动解析 */
            as_ser_segment_load_init(port, can_msg);
        break;

        case e_clt_up_init:         /* 段上传启动解析 */
            as_ser_segment_up_init(port, can_msg);
        break;

        case e_clt_up:              /* 分段上传解析 */
            as_ser_segment_up(port, can_msg); 
        break;

        case e_clt_stop_cmd:        /* sdo终止协议 */
            ret = sdo_stop(can_msg);
        break;
		
        case e_clt_blk_up:          /* 块上传解析 */

        break;
		
        case e_clt_blk_load:        /* 块下载解析 */
            as_ser_block_load(port, can_msg);   

        break;
		
        default :

        break;
    }
    
    return ret ;
}

/**
 * @brief 客户端与服务器解释
 * @return int8_t 
 */
static int8_t sdo_process(uint8_t port, ucan_frame *can_msg)
{
    int8_t ret = 0 ;
    
    if(SDO_RX_ID == can_msg->byte.m_cob_id.byte.m_FUN_ID)
    {
        ret = ser_sdo_process(port,can_msg);
    }
    else
    {
        ret = clt_sdo_process(port,can_msg);
    }
    return ret ;
}

/**
 * @brief 检测当前节点
 * 
 * @param comp_type 
 * @param can_msg 
 * @return int8_t 
 */ 
static int8_t sdo_analysis(uint16_t comp_type,ucan_frame *can_msg)
{
    int8_t ret = 0 ;
    int32_t err_code = 0 ;
    sindex m_index;

    if(comp_type == CUR_MASTER_ID)
    {
        ret = sdo_process(e_comp_mc,can_msg);
    }
    else
    {
        err_code = e_visit_obj_err;
        m_index.index = htons(can_msg->byte.data.type1.m_index) ;
        m_index.subindex = can_msg->byte.data.type1.m_subindex ;
        if(SDO_TX_ID == can_msg->byte.m_cob_id.byte.m_FUN_ID)
        {
            ret = send_err(SDO_RX_ID,can_msg->byte.m_cob_id.byte.m_SA_ID,m_index,err_code);
        }
        else
        {
            ret = send_err(SDO_TX_ID,can_msg->byte.m_cob_id.byte.m_SA_ID,m_index,err_code);
        }
    }
    return ret ;
}

/**
 * @brief 解析COB-ID类型
 * 
 * @return int8_t 
 */
int8_t protocol_analysis(ucan_frame *can_pkg)
{
    ucan_frame *can_msg = can_pkg ;
    int8_t ret = CAN_RC_OK ;
    uint16_t FUN_ID, NODE_ID;

    if(can_msg->byte.m_extend == CAN_ID_STD)  
    {
        /* 将标准帧转化为扩展帧 */
        FUN_ID = (uint16)can_msg->std_byte.m_cob_id.byte.m_FUN_ID << 7;
        NODE_ID = (uint16)can_msg->std_byte.m_cob_id.byte.m_Node_ID;
        can_msg->byte.m_cob_id.byte.m_FUN_ID = FUN_ID;
        can_msg->byte.m_cob_id.byte.m_DA_ID = NODE_ID;
    }

	/* 解析SDO数据帧 */
    if(SDO_RX_ID == can_msg->byte.m_cob_id.byte.m_FUN_ID)
    {
        ret = sdo_analysis(can_msg->byte.m_cob_id.byte.m_DA_ID,can_msg);
    }
    else if(SDO_TX_ID == can_msg->byte.m_cob_id.byte.m_FUN_ID)
    {
        ret = sdo_analysis(can_msg->byte.m_cob_id.byte.m_DA_ID,can_msg);
    }

    return ret ;
}


